
	area	main,	code,	readonly
		
NVIC_INT_CTRL   EQU     0xE000ED04                              ; Interrupt control state register.
NVIC_SYSPRI14   EQU     0xE000ED22                              ; System priority register (priority 14).
NVIC_PENDSV_PRI EQU           0xFF                              ; PendSV priority value (lowest).
NVIC_PENDSVSET  EQU     0x10000000                              ; Value to trigger PendSV exception.
	
OsTaskSwitch
	export	OsTaskSwitch
	LDR	R0, =NVIC_INT_CTRL
	LDR	R1, =NVIC_PENDSVSET
	STR	R1, [R0]
	BX	LR
			
	EXTERN  osTcbCurrent
	EXTERN  osTcbNext
	EXTERN  MyTaskStk
PendSV_Handler
	export	PendSV_Handler
        ;STMFD   SP!, {R0-R12}
        STMFD   SP!, {R4-R11}
	;     Push current CPSR
        ;MRS	R4,  xPSR
	;push	{R4}
	; osTcbCurrent->stk=SP;
        LDR     R4, =osTcbCurrent         	
        LDR     R5, [R4]
        str     R13, [R5]
	; SP = osTcbNext->stk;
        LDR     R6, =osTcbNext
        LDR     R7, [R6]
        LDR     R13, [R7]
	; osTcbCurrent = osTcbNext;
	str	R7,[R4]
	;    Pop new task's CPSR
        ;pop	{R4}
        ;MSR     xPSR, R4
        ;LDMFD   SP!, {R0-R12}
        LDMFD   SP!, {R4-R11}
	bx	lr
		
	area	osTask,	code,	readonly
	EXTERN  osTcbCurrent
OsTaskToCurrent
	export	OsTaskToCurrent
	; SP = osTcbCurrent->stk;
        LDR     R4, =osTcbCurrent
        LDR     R5, [R4]
        LDR     SP, [R5]
	
        LDMFD   SP!, {R4-R11}
        LDMFD   SP!, {R0-R3,R12}
	;    Pop new task's CPSR
        ;pop	{R4}
        ;MSR     xPSR, R4

        LDMFD   SP!, {lr}
        LDMFD   SP!, {pc}
	
	end
